
// CLEARLY OBVIOUS

/*
 *  ColorDetection.cpp
 *  VideoTilraun
 *
 *  Created by Ívar Björn Hilmarsson on 2.5.2009.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "ColorDetection.h"

ColorDetection::ColorDetection() {
	source			= 0;
	dest			= 0;
	workingCopy		= 0;
	imgCopy			= 0;
	capture			= cvCaptureFromCAM( 0 ); 
}

ColorDetection::~ColorDetection() {
	cvReleaseImage( &source );
	cvReleaseImage( &dest );
	cvReleaseImage( &workingCopy );
	cvReleaseImage( &imgCopy );
	cvReleaseCapture( &capture ); 	
}

void ColorDetection::prufa() {
#ifdef DEBUG
	cvNamedWindow(WINDSRC, 1);
	cvNamedWindow(WINDDST, 1);
	cvMoveWindow(WINDDST, 745, 100);
#endif
	while (true) {
		
		if (dest != NULL ) {
			cvReleaseImage( &dest );
		}
		
		if ( workingCopy != NULL ) {
			cvReleaseImage( &workingCopy );
		}
		
		if ( imgCopy != NULL ) {
			cvReleaseImage( &imgCopy );
		}
		
		source = cvQueryFrame( capture );
		
		dest = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		
		workingCopy = cvCloneImage(source);
		
		
		// Blurrum myndirnar til að minka false positives
		cvSmooth(workingCopy, workingCopy, CV_GAUSSIAN,9,9);
		cvSmooth(dest, dest, CV_GAUSSIAN,9,9);
		
		imgCopy = cvCreateImage(cvGetSize(source), source->depth, source->nChannels);
		//imgCopy = cvCloneImage(source);
		cvCopyImage(source, imgCopy);
		
		//detectHSV(imgCopy, workingCopy);
		greenHSV(imgCopy, workingCopy);
		
		cvCvtColor(workingCopy, dest, CV_BGR2GRAY);
		
		// Breytum myndinni í binary mynd
		CvScalar s;
		
		for( int i = 0; i < ((dest->height-1)*(dest->width)-1)-1; ++i) {
			s = cvGet1D(dest, i);
			if ( s.val[0] != 0){
				s.val[0] = 255;
			}
			else {
				continue;
			}
			cvSet1D(dest, i, s);
		}
		
		detectBlobs(dest, source);
		
#ifdef DEBUG		
		//cvReleaseCapture( &capture );
		cvShowImage(WINDSRC, source);
		cvShowImage(WINDDST, dest);
		//cvShowImage(WINDDST, workingCopy);
#endif
		
		c = cvWaitKey( 1 );
		if( (char)c == ESC ) {
			cvDestroyAllWindows();
			break;
		}
		
	}
	
}

void ColorDetection::grasBurt() {
	
	cvNamedWindow(WINDSRC, 1);
	cvNamedWindow(WINDDST, 1);
	cvMoveWindow(WINDDST, 745, 100);
	
	while ( true ) {
		
		if (dest != NULL ) {
			cvReleaseImage( &dest );
		}
		
		if ( workingCopy != NULL ) {
			cvReleaseImage( &workingCopy );
		}
		
		if ( imgCopy != NULL ) {
			cvReleaseImage( &imgCopy );
		}
		
		
		if ( source == NULL ) {
			//source = cvLoadImage("grasmbl.TIFF", 1 );
			//source = cvLoadImage("grasmbl1.TIFF", 1 );
			//source = cvLoadImage("grasmbl2.TIFF", 1 );
			//source = cvLoadImage("grasmbl3.TIFF", 1 );
			//source = cvLoadImage("grasmbl4.TIFF", 1 );
			//source = cvLoadImage("IMG1.JPG", 1 );
			source = cvLoadImage("mblInni.TIFF", 1);
		}
		
		dest = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		
		workingCopy = cvCloneImage(source);
		
		// Blurrum myndirnar til að minka false positives
		cvSmooth(workingCopy, workingCopy, CV_GAUSSIAN,9,9);
		cvSmooth(dest, dest, CV_GAUSSIAN,9,9);
		imgCopy = cvCreateImage(cvGetSize(source), source->depth, source->nChannels);
		cvCopyImage(source, imgCopy);
		
		greenHSV(imgCopy, workingCopy);
		
		//histo(workingCopy);
		CvScalar s;
		cvCvtColor(workingCopy, dest, CV_BGR2GRAY);
		for( int i = 0; i < ((dest->height-1)*(dest->width-1))-1; ++i){
			s = cvGet1D(dest, i);
			if ( s.val[0] != 0){
				s.val[0] = 255;
			}
			else {
				continue;
			}
			cvSet1D(dest, i, s);
		}
		
		detectBlobs(dest, source);
		
		//cvReleaseCapture( &capture );
		cvShowImage(WINDSRC, source);
		cvShowImage(WINDDST, dest);
		//cvShowImage(WINDDST, workingCopy);
		
		c = cvWaitKey( 10000 );
		if( (char)c == ESC ) {
			cvDestroyAllWindows();
			return;
		}
		
	}
	
}

void ColorDetection::histo(IplImage* img) {
	IplImage* back_img = cvCreateImage( cvGetSize(img),IPL_DEPTH_8U, 1);
	IplImage* hsv = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 3 );
	cvCvtColor( img, hsv, CV_BGR2HSV );
	
	IplImage* h_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* s_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* v_plane = cvCreateImage( cvGetSize( img ), 8, 1 );
	IplImage* planes[] = { h_plane, s_plane };
	cvCvtPixToPlane( hsv, h_plane, s_plane, v_plane, 0 );
	
	// Build and fill the histogram
	int h_bins = 30, s_bins = 32;
	CvHistogram* hist;
	{
		int hist_size[] = { h_bins, s_bins };
		float h_ranges[] = { 0, 180 };
		float s_ranges[] = { 0, 255 };
		float* ranges[] = { h_ranges, s_ranges };
		hist = cvCreateHist( 2, hist_size, CV_HIST_ARRAY, ranges, 1 );
	}
	cvCalcHist( planes, hist, 0, 0 ); // Compute histogram
	cvNormalizeHist( hist, 20*255 ); // Normalize it
	
	cvCalcBackProject( planes, back_img, hist );// Calculate back projection
	cvNormalizeHist( hist, 1.0 ); // Normalize it
	
	
	
	// Create an image to visualize the histogram
	int scale = 10;
	IplImage* hist_img = cvCreateImage( cvSize( h_bins * scale, s_bins * scale ), 8, 3 );
	cvZero ( hist_img );
	
	// populate the visualization
	float max_value = 0;
	cvGetMinMaxHistValue( hist, 0, &max_value, 0, 0 );
	
	for( int h = 0; h < h_bins; h++ ){
		for( int s = 0; s < s_bins; s++ ){
			float bin_val = cvQueryHistValue_2D( hist, h, s );
			int intensity = cvRound( bin_val * 255 / max_value );
			cvRectangle( hist_img, cvPoint( h*scale, s*scale ),
						cvPoint( (h+1)*scale - 1, (s+1)*scale - 1 ),
						CV_RGB( intensity, intensity, intensity ),
						CV_FILLED );
		}
	}
	cvNamedWindow("smu", 1);
	cvShowImage("smu", hist_img);
	cvReleaseImage( &h_plane );
	cvReleaseImage( &s_plane );
	cvReleaseImage( &v_plane );
	cvReleaseImage( &back_img );
	cvReleaseImage( &hsv );
	cvReleaseHist(&hist);
}

void ColorDetection::greenHSV(IplImage* frame, IplImage* HSVFrame) {
	//Breytur fyrir for lykkjurnar
	int i,j;
	
	/*Geymum 2D hnit fyrir BGR liti í hverjum pixel í myndinni */
	CvScalar s, cval;
	//*/
	
	// Geymi upplýsingar og gögn um myndina
	int height = 0;
	int width  = 0;
	uchar *imageData;
	int steps, channels;
	
	//Upper og lower bound fyrir rautt hue
	//double hlower=175,hupper=5; //Finnur skærrautt.
	//double hlower=145,hupper=35; //Orginal 
	double hlower=90,hupper=35;// Reina að finna dekkra rautt.
	
	//Saturn threshold
	height = frame->height;
	width = frame->width;
	int sthreshold = 140;
	int sthresholdUpper = 200;
	for(i=0;i<(height);i++) {
		for(j=0;j<(width);j++) {
			cval = cvGet2D( frame, i, j );
			if( cval.val[0] > 100 && cval.val[1] > 100 && cval.val[2] > 100) {
				cval.val[0] = 0;
				cval.val[1] = 180;
				cval.val[2] = 0;
			}
			else if ( cval.val[0] > 50 && cval.val[1] < 100 && cval.val[2] < 100) {
				cval.val[0] = 0;
				cval.val[1] = 180;
				cval.val[2] = 0;
			}
			else if ( cval.val[0] < 50 && cval.val[1] < 50 && cval.val[2] < 50) {
				cval.val[0] = 0;
				cval.val[1] = 180;
				cval.val[2] = 0;	
			}
			cvSet2D( frame, i, j, cval );
		}
	}
	cvNamedWindow("jonas", 1);
	cvShowImage("jonas", frame);
	//Breytum RGB myndinni í HSV
	cvCvtColor( frame ,HSVFrame ,CV_RGB2HSV );
	/* Smooting and Gaussian filtering * /
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN );
	 cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN );
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 //*/
	
	imageData	= (uchar *)HSVFrame->imageData;
	
	height		= HSVFrame->height;
	width		= HSVFrame->width;
	steps		= HSVFrame->widthStep;
	channels	= HSVFrame->nChannels;
	midX		= height/2;
	midY		= width/2;
	
	/* Síðasta færibreytan segir til hversu oft við viljum keyra dilation eða erosion * /
	 cvErode(frame,frame,0,6);
	 cvDilate( frame,frame,0,7);
	 //*/
	
	/*Notum HSV og finnum rauðan lit í myndinni */
	for(i=0;i<(height);i++) {
		for(j=0;j<(width);j++) {
			cval = cvGet2D( frame, i, j );
			if((imageData[(i)*steps+j*channels]<=hlower) && (imageData[(i)*steps+j*channels]>=hupper)) { 
				s = cvGet2D( HSVFrame, i, j );
				cval = cvGet2D( frame, i, j );
				if((imageData[(i)*steps+j*(channels)+1])>sthreshold && (imageData[(i)*steps+j*(channels)+1]) < sthresholdUpper && cval.val[0] < 60 && cval.val[1] < 60 && cval.val[2] > 70 ) {
					/*imageData[i*steps+j*channels]=10;
					 s.val[0]=340;
					 s.val[1]=80;
					 s.val[2]=100;*/
					imageData[i*steps+j*channels]=255;
					s.val[0]=255;
					s.val[1]=255;
					s.val[2]=255;
				}
				else {
					/* Ef við finnum ekki rautt hue þá setjum við núll í gildið */
					//imageData[i*steps+j*channels]=0;
					s.val[0]=0;
					s.val[1]=0;
					s.val[2]=0;
				}
				// Vistum þá pixels sem við höfum átt við.
				cvSet2D( HSVFrame, i, j, s );
			}//Ytra if endar
			//cvSet2D( source, i, j, cval );
		}//Innra for endar
	}//Ytra for endar		
	
	/* Síðasta færibreytan segir til hversu oft við viljum keyra dilation eða erosion */
	cvErode(HSVFrame,HSVFrame,0,10);
	cvDilate( HSVFrame,HSVFrame,0,10);
	//*/
	
	/*
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 cvSmooth( HSVFrame, HSVFrame, CV_BLUR );
	 cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN, 9, 9 );
	 cvSmooth( HSVFrame, HSVFrame, CV_BLUR );
	 cvSmooth( HSVFrame, HSVFrame, CV_BILATERAL, 9, 9 );
	 //cvSmooth( dest, dest, CV_BILATERAL, 9, 9 );
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 cvCvtColor( HSVFrame ,HSVFrame ,CV_HSV2BGR );
	 //*/
	
}


void ColorDetection::start() {
	int i = 0;
	while (i < 3) {
		
		if (dest != NULL ) {
			cvReleaseImage( &dest );
		}
		
		if ( workingCopy != NULL ) {
			cvReleaseImage( &workingCopy );
		}
		
		if ( imgCopy != NULL ) {
			cvReleaseImage( &imgCopy );
		}
		
		source = cvQueryFrame( capture );
		
		dest = cvCreateImage(cvGetSize(source), IPL_DEPTH_8U, 1);
		
		workingCopy = cvCloneImage(source);
		
		
		// Blurrum myndirnar til að minka false positives
		cvSmooth(workingCopy, workingCopy, CV_GAUSSIAN,9,9);
		cvSmooth(dest, dest, CV_GAUSSIAN,9,9);
		
		imgCopy = cvCloneImage(source);
		
		detectHSV(imgCopy, workingCopy);
		
		cvCvtColor(workingCopy, dest, CV_BGR2GRAY);
		
		//blobFinder(source, workingCopy);
		detectBlobs(dest, source);
		
		//cvReleaseCapture( &capture );
		
		c = cvWaitKey( 1 );
		i++;
	}
}

void ColorDetection::detectBlobs(IplImage* frame, IplImage* finalFrame){
	
	int blobCounter = 0;
	map<unsigned int, blob> blobs;
	
    unsigned char threshold = 135;
	
	// lámarks stærðin á blob sem við viljum skoða
	int blobThreshold = 1800;
	//int blobThreshold = 5000;
	int upperThreshold = 35000;
	
    vector< vector<lineBlob> > imgData(frame->width);
	
	for(int row = 0; row < frame->height; ++row){
		for(int column = 0; column < frame->width; ++column){
			
			int asni = frame->imageData[(row*frame->width) + column];
			if (asni > 150) printf("char er %d ",asni);
			unsigned char byte = (unsigned char) frame->imageData[(row*frame->width) + column];
			
			if(byte >= threshold){
				int start = column;
				
				for(;byte >= threshold; byte = (unsigned char) frame->imageData[(row*frame->width)+ column], ++column);
				
				int stop = column-1;
				lineBlob lineBlobData = {start, stop, blobCounter, false};
				imgData[row].push_back(lineBlobData);
				blobCounter++;
			}
		}
	}
	
	/* Skoða hvort blob í línu snertist og snerti í næstu línu */
	for(int row = 0; row < imgData.size(); ++row){
		for(int entryLine1 = 0; entryLine1 < imgData[row].size(); ++entryLine1){
			for(int entryLine2 = 0; entryLine2 < imgData[row+1].size(); ++entryLine2){
				if(!((imgData[row][entryLine1].max < imgData[row+1][entryLine2].min) || (imgData[row][entryLine1].min > imgData[row+1][entryLine2].max))){
					if(imgData[row+1][entryLine2].attached == false){
						imgData[row+1][entryLine2].blobId = imgData[row][entryLine1].blobId;
						imgData[row+1][entryLine2].attached = true;
					}
					else{
						imgData[row][entryLine1].blobId = imgData[row+1][entryLine2].blobId;
						imgData[row][entryLine1].attached = true;
					}
				}
			}
		}
	}
	
	// Sortera og grúppa saman blobs
	for(int row = 0; row < imgData.size(); ++row){
		for(int entry = 0; entry < imgData[row].size(); ++entry){
			if(blobs.find(imgData[row][entry].blobId) == blobs.end()){
				blob blobData = {{imgData[row][entry].min, row}, {imgData[row][entry].max, row}, {0,0}};
				blobs[imgData[row][entry].blobId] = blobData;
			}
			else{
				if(imgData[row][entry].min < blobs[imgData[row][entry].blobId].min.x)
					blobs[imgData[row][entry].blobId].min.x = imgData[row][entry].min;
				else if(imgData[row][entry].max > blobs[imgData[row][entry].blobId].max.x)
					blobs[imgData[row][entry].blobId].max.x = imgData[row][entry].max;
				if(row < blobs[imgData[row][entry].blobId].min.y)
					blobs[imgData[row][entry].blobId].min.y = row;
				else if(row > blobs[imgData[row][entry].blobId].max.y)
					blobs[imgData[row][entry].blobId].max.y = row;
			}
		}
	}
	
	// reiknum út miðpunkt
	for(map<unsigned int, blob>::iterator i = blobs.begin(); i != blobs.end(); ++i){
		(*i).second.center.x = (*i).second.min.x + ((*i).second.max.x - (*i).second.min.x) / 2;
		(*i).second.center.y = (*i).second.min.y + ((*i).second.max.y - (*i).second.min.y) / 2;
		
		int size = ((*i).second.max.x - (*i).second.min.x) * ((*i).second.max.y - (*i).second.min.y);
		
		// Teiknum kassa utanum svæðið sem við fundum og skoðum ROI til að sjá hvaða rör þetta gæti verið.
		if(size > blobThreshold && size < upperThreshold) {
#ifdef DEBUG
			//CvFont font;
			//cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0, 0, 1, CV_AA);
			
			//char textBuffer[128];
			
			//cvLine(finalFrame, cvPoint((*i).second.center.x - 5, (*i).second.center.y), cvPoint((*i).second.center.x + 5, (*i).second.center.y), cvScalar(0, 0, 153), 1);
			//cvLine(finalFrame, cvPoint((*i).second.center.x, (*i).second.center.y - 5), cvPoint((*i).second.center.x, (*i).second.center.y + 5), cvScalar(0, 0, 153), 1);
#endif			
			//gildiX = (*i).second.max.x;
			//gildiY = (*i).second.max.y;
			
			// Segjum rovernum hvernig á að nálgast rörið.
			//direction();
			
			/* Setjum ROI fyrir það svæði sem við notum til að auðkenna hvaða geimvera þetta er sem við erum að skoða */
			cvSetImageROI(finalFrame, cvRect((*i).second.min.x, (*i).second.min.y, (*i).second.max.x, (*i).second.max.y) );
			//alienRecognition(finalFrame);
			
			//Leita af svörtum punkti.
			identifyPipe(finalFrame, ((*i).second.max.x - (*i).second.min.x));
			
			// Verður alltaf að losa ROI af mynd 
			cvResetImageROI(finalFrame);
			//*/
#ifdef DEBUG
			//sprintf(textBuffer, "(%d, %d)", (*i).second.center.x, (*i).second.center.y);
			//cvPutText(finalFrame, textBuffer, cvPoint((*i).second.center.x + 5, (*i).second.center.y - 5), &font, cvScalar(0, 0, 153));
			cvRectangle(finalFrame, cvPoint((*i).second.min.x, (*i).second.min.y), cvPoint((*i).second.max.x, (*i).second.max.y), cvScalar(0, 0, 153), 1);
#endif			
			// Prenta út miðju punktinn (bara fyrir debugging)
			//cout << "(" << (*i).second.center.x << ", " << (*i).second.center.y << ")" << endl;
		}
	}
}

void ColorDetection::detectHSV(IplImage* frame, IplImage* HSVFrame) {
	//Breytur fyrir for lykkjurnar
	int i,j;
	
	/*Geymum 2D hnit fyrir BGR liti í hverjum pixel í myndinni */
	CvScalar s, cval;
	//*/
	
	// Geymi upplýsingar og gögn um myndina
	int height = 0;
	int width  = 0;
	uchar *imageData;
	int steps, channels;
	
	//Upper og lower bound fyrir rautt hue
	//double hlower=175,hupper=5; //Finnur skærrautt.
	//double hlower=145,hupper=35; //Orginal 
	double hlower=145,hupper=35;// Reina að finna dekkra rautt.
	
	//Saturn threshold
	int sthreshold=140;
	
	//Breytum RGB myndinni í HSV
	cvCvtColor( frame ,HSVFrame ,CV_RGB2HSV );
	/* Smooting and Gaussian filtering * /
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN );
	 cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN );
	 cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	 //*/
	
	imageData	= (uchar *)HSVFrame->imageData;
	
	height		= HSVFrame->height;
	width		= HSVFrame->width;
	steps		= HSVFrame->widthStep;
	channels	= HSVFrame->nChannels;
	midX		= height/2;
	midY		= width/2;
	
	/* Síðasta færibreytan segir til hversu oft við viljum keyra dilation eða erosion */
	cvErode(frame,frame,0,7);
	cvDilate( frame,frame,0,8);
	//*/
	
	int average = 0;
	
	/*Notum HSV og finnum rauðan lit í myndinni */
	for(i=0;i<(height);i++) {
		for(j=0;j<(width);j++) {
			if((imageData[(i)*steps+j*channels]<=hlower) && (imageData[(i)*steps+j*channels]>=hupper)) { 
				s = cvGet2D( HSVFrame, i, j );
				cval = cvGet2D( frame, i, j );
				average = (cval.val[0] + cval.val[1] + cval.val[2])/3;
				if (average > 160) {
					/* Ef við finnum ekki rautt hue þá setjum við núll í gildið */
					imageData[i*steps+j*channels]=0;
					s.val[0]=0;
					s.val[1]=0;
					s.val[2]=0;
				}
				else if((imageData[(i)*steps+j*(channels)+1])>sthreshold && cval.val[0] < 90 && cval.val[1] < 90 && cval.val[2] > 70 && average < 160) {
					imageData[i*steps+j*channels]=255;
					s.val[0]=205;
					s.val[1]=155;
					s.val[2]=255;
				}
				else {
					/* Ef við finnum ekki rautt hue þá setjum við núll í gildið */
					imageData[i*steps+j*channels]=0;
					s.val[0]=0;
					s.val[1]=0;
					s.val[2]=0;
				}
				// Vistum þá pixels sem við höfum átt við.
				cvSet2D( HSVFrame, i, j, s );
			}//Ytra if endar
		}//Innra for endar
	}//Ytra for endar		
	
	/* Síðasta færibreytan segir til hversu oft við viljum keyra dilation eða erosion */
	cvErode(HSVFrame,HSVFrame,0,9);
	cvDilate( HSVFrame,HSVFrame,0,10);
	//*/
	
	cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	cvSmooth( HSVFrame, HSVFrame, CV_BLUR );
	cvSmooth( HSVFrame, HSVFrame, CV_MEDIAN );
	cvSmooth( HSVFrame, HSVFrame, CV_BLUR );
	cvSmooth( dest, dest, CV_BILATERAL );
	cvSmooth( HSVFrame, HSVFrame, CV_GAUSSIAN, 9, 9 );
	cvCvtColor( HSVFrame ,HSVFrame ,CV_HSV2BGR );
	
}

void ColorDetection::direction() {
	char* stefna = "";
	if ((midX - gildiX) > 0 ) {
		stefna = " Beygðu til Vinstri helvítis róbots helvítis drasl";
	}
	else {
		stefna = " Beygðu til Hægri helvítis róbots helvítis drasl";
	}
	
	cout << stefna << endl;
	if ((midY - gildiY) > 0 ) {
		stefna = " Keyrðu áfram helvítis róbots helvítis drasl";
	}
	else {
		stefna = " Þú ert of fucking nálægt helvítis róbots helvítis drasl";
	}
	cout << stefna << endl;
}

void ColorDetection::identifyPipe(IplImage* frame, int breidd) {
	cout << "Breidd er: " << breidd << endl;
	if (breidd > 74 && breidd < 130 && frame != NULL ) {	
		CvScalar s;
#ifdef DEBUG
		cvNamedWindow("What was found", 1);
		cvShowImage("What was found", frame);
		cvMoveWindow("What was found", 100, 500);
		cout << "Breiddin á rörinu er: " << breidd <<  endl;
#endif
		bool erSvartur = true;
		int blackCount = 0, blackCountTotal = 0, blackTapeCount = 0;
		int width = 0;
		if (breidd > frame->width){
			width = frame->width;
		}
		else {
			width = breidd;
		}
		if ( frame->height < 11) {
			return;
		}
		
		// Leitum af svörtum punktum í mynd
		for(int j = 5; j < 11; ++j){
			blackCount = 0;
			for (int i = 0; i < width-1; ++i) {
				//for (int i = 0; i < breidd-1; ++i) {
				s = cvGet2D(frame, j, i);
				if ( (s.val[0] + s.val[1] + s.val[2])/3 < 50) { 
					blackCount++;
					if (erSvartur) {
						blackTapeCount++;
						erSvartur = false;
					}
				}
				else {
					erSvartur = true;
				}
				
			}
			blackCountTotal += blackCount;
		}
		int blackAverage = blackCountTotal/(11-5);
		int blackTapeAverage = blackTapeCount/(11-5);
#ifdef DEBUG		
		cout << "Total black count: " << blackCountTotal << " Average: " << blackAverage << endl;
		cout << "Tape count er: " << blackTapeCount << " Average: " << blackTapeAverage << endl;
#endif	
		// Segjum hvaða rör við fundum
		if (blackTapeAverage == 1 && blackAverage > 8) {
			cout << "Fann rör 1" << endl;
		}
		else if (blackTapeAverage == 2) {
			if(blackAverage > 20) {
				cout << "Fann rör 2" << endl;
			}
			else {
				cout << "Fann rör 1" << endl;
			}
		}
		else if (blackTapeAverage == 3) {
			cout << "Fann rör 2" << endl;
		}
		else {
			cout << "Fann rör 0" << endl;
		}
	}
	else {
		if (breidd < 75) {
			cout << "Rörið er ekki nógu nálægt til að þekkja það." << endl;
		}
		else {
			cout << "Rörið er of nálægt til að þekkja það." << endl;
		}
	}
}
